#include "geometr.h"

#include <stdlib.h>
#include <math.h>


/*
 * uint16_t distance(...)
 * Returns the distance between P0(x0,y0) and P1(x1,y1)
 */

uint16_t distance (uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1) {
 return (uint16_t) sqrt( (x0-x1)*(x0-x1) + (y0-y1)*(y0-y1) );
}

void
line_3D(int16_t x1, int16_t y1, int16_t z1, int16_t x2, int16_t y2, int16_t z2)
{
    int16_t i, dx, dy, dz, l, m, n, x_inc, y_inc, z_inc, err_1, err_2, dx2, dy2, dz2;

    dx = x2 - x1;
    dy = y2 - y1;
    dz = z2 - z1;
    x_inc = (dx < 0) ? -1 : 1;
    l = abs(dx);
    y_inc = (dy < 0) ? -1 : 1;
    m = abs(dy);
    z_inc = (dz < 0) ? -1 : 1;
    n = abs(dz);
    dx2 = l << 1;
    dy2 = m << 1;
    dz2 = n << 1;

    if ((l >= m) && (l >= n)) {
        err_1 = dy2 - l;
        err_2 = dz2 - l;
        for (i = 0; i < l; i++) {
            putpixel(x1,y1,z1);
            if (err_1 > 0) {
                y1 += y_inc;
                err_1 -= dx2;
            }
            if (err_2 > 0) {
                z1 += z_inc;
                err_2 -= dx2;
            }
            err_1 += dy2;
            err_2 += dz2;
            x1 += x_inc;
        }
    } else if ((m >= l) && (m >= n)) {
        err_1 = dx2 - m;
        err_2 = dz2 - m;
        for (i = 0; i < m; i++) {
            putpixel(x1,y1,z1);
            if (err_1 > 0) {
                x1 += x_inc;
                err_1 -= dy2;
            }
            if (err_2 > 0) {
                z1 += z_inc;
                err_2 -= dy2;
            }
            err_1 += dx2;
            err_2 += dz2;
            y1 += y_inc;
        }
    } else {
        err_1 = dy2 - n;
        err_2 = dx2 - n;
        for (i = 0; i < n; i++) {
            putpixel(x1,y1,z1);
            if (err_1 > 0) {
                y1 += y_inc;
                err_1 -= dz2;
            }
            if (err_2 > 0) {
                x1 += x_inc;
                err_2 -= dz2;
            }
            err_1 += dy2;
            err_2 += dx2;
            z1 += z_inc;
        }
    }
    putpixel(x1,y1,z1);
}

void
arc(int16_t x1, int16_t y1,
    int16_t xmid, int16_t ymid
	// int8_t angle
	)
{
  int16_t 	i;
  int16_t 	X0 = x1,
			Y0 = y1,
			r = distance(X0, Y0, xmid, ymid),
			AngleOffset = 0;
			
  // Find angle offset
  for (i=0; i <= 360; i++)
  {
   X0 = xmid + r * cos(i / 57); // 57.29577);
   Y0 = ymid + r * sin(i / 57); // 57.29577);
  
   if (distance(X0, Y0, x1, y1) == 0)
   {
    AngleOffset = i;
	break;
   }
  }
  //if (angle < 0) AngleOffset = 360 - AngleOffset;
  
  // Plot arc
  for (i = AngleOffset ; i <= 360; i++)
  {
   X0 = xmid + r * cos(i / 57.29577);
   //if (angle > 0)   Y0 = ymid - r * sin(i / 57);
   //else			     
   Y0 = ymid + r * sin(i / 57);
   
   putpixel (X0, Y0, Z.Pos);
   
   if (distance(X0,Y0, x1,y1) <= 1) break;
  }
}
